close all;
clear all;

%V = load('~/Dropbox/Bonobo-ASLab-Share/steveshare/video-based/blue_video.txt');
%M = load('~/Dropbox/Bonobo-ASLab-Share/steveshare/video-based/blue_mapper.txt');

V = load('~/Dropbox/Bonobo-ASLab-Share/steveshare/video-based/desktop_video.txt');
M = load('~/Dropbox/Bonobo-ASLab-Share/steveshare/video-based/desktop_mapper.txt');


%V = load('~/videoslam.txt');
%M = load('~/mapper.txt');

scale_factor = 0.2;
%stop = 1384482877.0;
stop = 1384482886.0;
%restart = 1384482880.0; 
restart = 1384482887.0;

Vi(:,1) = M(:,1);
Vi(:,2) = M(:,2);

for iii = 1:6
	Vi(:,iii+2) = interp1(V(:,1), V(:,iii+2), M(:,1));
end

for iii = 7:size(V,2)-2
	Vi(:,iii+2) = interp1(V(:,1), V(:,iii+2), M(:,1));
end

error(:,1) = M(:,1);
error(:,2) = M(:,2);
error(:,3) = ((Vi(:,3)-M(:,3)).^2 + (Vi(:,4)-M(:,4)).^2 + (Vi(:,5)-M(:,5)).^2).^0.5;

% to calculate quaternion angle

error(:,4) = min(abs(acos(2*( Vi(:,6).*M(:,6) + Vi(:,7).*M(:,7) + Vi(:,8).*M(:,8) + Vi(:,9).*M(:,9) ).^2 - 1)), pi/2-abs(acos(2*( Vi(:,6).*M(:,6) + Vi(:,7).*M(:,7) + Vi(:,8).*M(:,8) + Vi(:,9).*M(:,9) ).^2 - 1)));

sc = 1.0;
%plot(error(:,1)-min(error(:,1)), error(:,4)*sc,'b');
%hold on;
figure(1);
plot(error(:,1)-min(error(:,1)), error(:,3),'r', 'LineWidth', 2);

%plot(Vi(:,1)-min(error(:,1)), scale_factor + scale_factor*Vi(:,10)/max(Vi(:,10)),'g');
%plot(Vi(:,1)-min(error(:,1)), scale_factor + scale_factor*Vi(:,11)/max(Vi(:,11)),'m');
%plot(Vi(:,1)-min(error(:,1)), scale_factor + scale_factor*Vi(:,12)/max(Vi(:,12)),'b');
%plot(Vi(:,1)-min(error(:,1)), scale_factor + scale_factor*Vi(:,13)/max(Vi(:,13)),'c');
%plot(Vi(:,1)-min(error(:,1)), scale_factor + scale_factor*Vi(:,15)/max(Vi(:,15)),'c');
%plot(M(:,1), M(:,3),'k');

if 1
	%plot([stop-min(error(:,1)) stop-min(error(:,1))], [0.0 2*scale_factor], 'k');
	%plot([restart-min(error(:,1)) restart-min(error(:,1))], [0.0 2*scale_factor], 'k');
	%plot([0 max(V(:,1))-min(error(:,1))], [scale_factor scale_factor], 'k');
	%axis([1384482875 1384482895 0.0 scale_factor]);
	%axis([0 max(V(:,1))-min(error(:,1)) 0.0 2*scale_factor]);
	axis([0 30.0 0.0 0.2]);
end

ylabel('translation error (m)', 'fontsize', 20);
xlabel('time (s)', 'fontsize', 20);
%legend("proposed video-supported method", "conventional ICP-only method", "location", "northoutside")

figure(2);
%plot(Vi(:,1)-min(error(:,1)), scale_factor + scale_factor*Vi(:,10)/max(Vi(:,10)),'r');

plot(Vi(:,1)-min(error(:,1)), Vi(:,11)/max(Vi(:,11)),'m');
hold on;
plot(Vi(:,1)-min(error(:,1)), Vi(:,12)/max(Vi(:,12)),'b');
%plot(Vi(:,1)-min(error(:,1)), scale_factor + scale_factor*Vi(:,10)/max(Vi(:,16)),'g');

axis([0 30.0 0.0 1.0]);

ylabel('bundle-adjustment shift', 'fontsize', 20);
xlabel('time (s)', 'fontsize', 20);
legend("translation", "orientation", "location", "northoutside")
